package endcourseproject;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

public class Program {

	private static final float WHEEL_DIAMETER = 5.6F;
	private static final float TRACK_WIDTH = 11.5F;

	private static CompassSensor cs; // Only for use if PilotType = Compass
    private static Pilot _pilot;
	private static TouchSensor ts;
	private static UltrasonicSensor us;
	private static DirectionSensor ds;
	private static ZigBeeDevice zbd;
	private static RobotManager rm;
	private static Navigator navigator;
	
	private enum PilotType
	{
		Unknown,
		Compass,
		Tacho,
		Stationary,
		NUM_OF_TYPES
	}
	
	private static PilotType TypeOfPilot = PilotType.Unknown;
	
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
				
		// Initial user selection required
		userPilotSelect();

		// Handle user pilot selection
		if (TypeOfPilot == PilotType.Compass)
		{
			initCompassPilot();
		}
		else if(TypeOfPilot == PilotType.Tacho)
		{
			initTachoPilot();
		}
		else if(TypeOfPilot == PilotType.Stationary)
		{
			initStationaryPilot();
		}
		else
		{
			// Unknown => exit
			return;
		}
		
		// Create ZigBee interface and communication manager
		zbd = new ZigBeeDevice(SensorPort.S1);
		rm = new RobotManager(zbd);
		
		// Create navigator
		navigator = new Navigator(_pilot,rm, false);
		
		// Create sensors 
		ts = new TouchSensor(SensorPort.S4);
		ds = new DirectionSensor() {
			// Inline DirectionSensor implementation
			@Override
			public int getDirection() {
				return (int)navigator.getAngle();
				
			}
		};
		us = new UltrasonicSensor(SensorPort.S3);
		
		
		
		// Create and add behaviors to navigator
		RandomDrive rd = new RandomDrive();
		Avoid a = new Avoid(ts, ds);
		UltraAvoid ua = new UltraAvoid(us, ds);
		Flock f = new Flock(rm,ds);

		// Add travel behavior to robots with compass
		if( TypeOfPilot == PilotType.Compass)
		{
			Travel t = new Travel(180);	
			//navigator.addBehavior(t, 20);
		}
		
		// Add common behaviors to navigator
		navigator.addBehavior(a, 90);
		//navigator.addBehavior(ua, 50);
		navigator.addBehavior(f, 40);
		//navigator.addBehavior(rd, 10);

		
		// Start navigator thread
		navigator.start();
		
		
		// Down sample message transmit
		int transmit = 0;
		
		// Wait for exit!
		while (!Button.ESCAPE.isPressed()) {
			
			Common.sleep(50);
			if( transmit++ > 20 )
			{
				// Transmit every 50*20 ms = 1 second
				transmit = 0;
				rm.sendMessage();
			}
		}

		// Application exit
		return;
	}
	
	private static void incrementType()
	{
		switch(TypeOfPilot)
		{
		case Compass:
			TypeOfPilot = PilotType.Tacho;
			break;
		case Tacho:
			TypeOfPilot = PilotType.Stationary;
			break;
		case Stationary:
			TypeOfPilot = PilotType.Compass;
			break;
		default:
			TypeOfPilot = PilotType.Compass;
			break;
		}
	}
	
	private static void decrementType()
	{
		switch(TypeOfPilot)
		{
		case Compass:
			TypeOfPilot = PilotType.Stationary;
			break;
		case Tacho:
			TypeOfPilot = PilotType.Compass;
			break;
		case Stationary:
			TypeOfPilot = PilotType.Tacho;
			break;
		default:
			TypeOfPilot = PilotType.Compass;
			break;
		}
	}
	
	private static void userPilotSelect()
	{
		LCD.drawString("Select Pilot:", 0, 0);
		LCD.drawString("TachoPilot   ", 0, 1);
		LCD.drawString("ENTER to continue", 0, 2);
		LCD.drawString("ESC to exit", 0, 3);

		Common.beep();
		TypeOfPilot = PilotType.Tacho; // Default pilot is Compass
		
		while (!Button.ENTER.isPressed())
		{
			
			if( Button.LEFT.isPressed())
			{
				incrementType();
				while(Button.LEFT.isPressed() ) { Common.sleep(10); }
			}
			else if ( Button.RIGHT.isPressed())
			{
				decrementType();
				while(Button.RIGHT.isPressed() ) { Common.sleep(10); }
			}
		
			if( Button.ESCAPE.isPressed() )
			{
				TypeOfPilot = PilotType.Unknown;
				return;
			}
			
			switch(TypeOfPilot)
			{
			case Compass:
				LCD.drawString("CompassPilot    ", 0, 1);
				break;
			case Tacho:
				LCD.drawString("TachoPilot      ", 0, 1);
				break;
			case Stationary:
				LCD.drawString("StationaryPilot ", 0, 1);
				break;
			default:
				LCD.drawString("Unknown         ", 0, 1);
				break;
			}
			
			Common.sleep(5);
		}
		LCD.clear();
	
	}

	private static void initCompassPilot()
	{
		cs = new CompassSensor(SensorPort.S2);
		
		// nice we are a well equipped robot with a compass
		_pilot = new CompassPilot(cs, WHEEL_DIAMETER, TRACK_WIDTH, Motor.C, Motor.B); 

		LCD.drawString("Calibrating", 0, 0);
		CompassPilot cp = (CompassPilot)_pilot; 
		cp.calibrate();

		LCD.drawString("Rotating to north", 0, 0);
		cp.rotateTo(0, false);
		Common.beep();
		LCD.clear();
		Common.sleep(1000);
	}
	
	private static void initTachoPilot()
	{
		// we don't have a compass so we will have to make do with the Tacho
		_pilot = new TachoPilot(WHEEL_DIAMETER, TRACK_WIDTH, Motor.C, Motor.B);		
	}
	
	private static void initStationaryPilot()
	{
		_pilot = new StationaryPilot();
	}
}
